/*	Copyright 2007 - Xavier Baro (xbaro@cvc.uab.cat)

	This file is part of eapmlib.

    Eapmlib is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 3 of the License, or any 
	later version.

    Eapmlib is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "Cascade.h"
#include "AdaBoost.h"
#include "IntSamplesSet.h"
#include <time.h>

Evolutive::CCascade::CCascade(void) : CClassEnsemble()
{	
#ifndef USE_OPENCV
	OPENCV_ERROR_CLASS("CCascade");
#endif
}

Evolutive::CCascade::~CCascade(void)
{		
}

#ifdef USE_OPENCV
double Evolutive::CCascade::Apply(CData *InputData)
{	
	register int i;
	double ClassVal;
	Evolutive::CClassifier *pClass;
		
	for(i=0;i<GetNumClassifiers();i++)
	{
		//! Point to the classifier
		pClass=GetClassifier(i);

		//! Apply the classifier
		ClassVal=pClass->Apply(InputData);
		
		//! Apply the classifier
		if(m_StageThresholds.size()>0)
		{
			if(ClassVal<m_StageThresholds[i])
				return -1.0;
		}
		else
		{
			if(ClassVal<0.0)
				return -1.0;
		}
	}
	
	return 1.0;
}

void Evolutive::CCascade::Serialize(std::fstream &fs,bool Saving)
{
	int NumWeights;
	register int i;
	double Value;

	// Call the base class method
	CClassEnsemble::Serialize(fs,Saving);

	// Implement the specialized code
	if(Saving)
	{
		// Save the data to a file
		// Store the number of weights
		NumWeights=m_StageThresholds.size();
		fs.write((char*)&NumWeights,sizeof(int));

		// Store each weight
		for(i=0;i<NumWeights;i++)
		{			
			Value=m_StageThresholds[i];
			fs.write((char*)&Value,sizeof(double));
		}
	}
	else
	{
		// Load the data from a file
		// Read the number of weights
		fs.read((char*)&NumWeights,sizeof(int));

		// Remove old data		
		m_StageThresholds.clear();

		// Load the new weights
		for(i=0;i<NumWeights;i++)
		{		
			fs.read((char*)&Value,sizeof(double));
			m_StageThresholds.push_back(Value);
		}
	}	
}

double Evolutive::CCascade::GetStageThr(int Idx)
{
	if(Idx<0 || Idx>=m_StageThresholds.size())
		throw CEvolutiveLibException("Index out of bounds",__FILE__,__LINE__,"GetStageThr");

	return m_StageThresholds[Idx];
}

void Evolutive::CCascade::SetStageThr(int Idx,double Thr)
{
	if(Idx<0 || Idx>=m_StageThresholds.size())
		throw CEvolutiveLibException("Index out of bounds",__FILE__,__LINE__,"SetStageThr");

	m_StageThresholds[Idx]=Thr;
}

void Evolutive::CCascade::AddClassifier(CClassifier *Classifier)
{
	// Use a default threshold of 0
	AddClassifier(Classifier,0.0);
}

void Evolutive::CCascade::AddClassifier(CClassifier *Classifier,double Thr)
{
	// Set the weight of this classifier
	m_StageThresholds.push_back(Thr);

	// Set the classifier using the base class method
	CClassEnsemble::AddClassifier(Classifier);
}

double Evolutive::CCascade::FindThreshold(int NumVals,double *Values, int *Labels,Evolutive::THR_FIND_MODE Mode,double *ParW_HR,double *FA)
{
	double *SortedVector=NULL;
	register int i,j;
	double LastVal=0,ThrVal=0;
	int NegOK,NegFail,PosOK,PosFail;
	double PosW,NegW;
	double BestThr,BestErr;
	double AuxErr;

	// Check the parameters
	if(!Values || !Labels)
		throw CEvolutiveLibException("NULL pointer to values or labels",__FILE__,__LINE__,"FindThreshold");
	if(Mode==THRF_MIN_WERROR && !ParW_HR)
		throw CEvolutiveLibException("NULL pointer to a weights vector",__FILE__,__LINE__,"FindThreshold");
	if(Mode==THRF_HR && !ParW_HR)
		throw CEvolutiveLibException("NULL pointer to a Hit Ratio value",__FILE__,__LINE__,"FindThreshold");

	// Add all the elements to an auxiliary vector
	SortedVector=new double[NumVals];
	memcpy(SortedVector,Values,NumVals*sizeof(double));
	
	// Sort the values
	qsort(SortedVector,NumVals,sizeof(double),Evolutive::DblCompareAsc);

	// Test with each value
	LastVal=SortedVector[0]-fabs(SortedVector[0]*2.0)-1;
	ThrVal=LastVal;
	BestErr=100000;
	BestThr=0;
	for (i=0;i<NumVals;i++)
	{
		// Skip repeated values
		if (LastVal==SortedVector[i])
			continue;

		// Pick a threshold value between the last value and the new
		ThrVal=LastVal+(SortedVector[i]-LastVal)/2.0;

		// Store the new value
		LastVal=SortedVector[i];

		// Initialize the variables
		NegOK=0;
		NegFail=0;
		PosOK=0;
		PosFail=0;
		PosW=0.0;
		NegW=0.0;

		// Get the error
		for (j=0;j<NumVals;j++)
		{
			if (Values[j]<ThrVal && Labels[j]<0)
				NegOK++;
			if (Values[j]<ThrVal && Labels[j]>0)
			{
				PosFail++;
				if(ParW_HR && Mode==THRF_MIN_WERROR)
					PosW+=ParW_HR[j];
			}
			if (Values[j]>=ThrVal && Labels[j]<0)
			{
				NegFail++;
				if(ParW_HR && Mode==THRF_MIN_WERROR)
					NegW+=ParW_HR[j];
			}
			if (Values[j]>=ThrVal && Labels[j]>0)
				PosOK++;
		}

		// Process the error value
		switch(Mode)
		{
		case THRF_MIN_WERROR:
			AuxErr=PosW+NegW;
			break;
		case THRF_MIN_ERROR:
			AuxErr=static_cast<double>(PosFail+NegFail)/static_cast<double>(NumVals);
			break;
		case THRF_MIN_BERR:
			AuxErr=static_cast<double>(PosFail)/static_cast<double>(PosOK+PosFail);
			AuxErr+=static_cast<double>(NegFail)/static_cast<double>(NegOK+NegFail);
			AuxErr/=2.0;
			break;
		case THRF_HR:
			AuxErr=static_cast<double>(PosOK)/static_cast<double>(PosOK+PosFail);
			// If the hit ratio is smaller than the required return the last valid threshold value and finish 
			if(AuxErr<ParW_HR[0])
			{
				// Remove the auxiliary vector
				delete[] SortedVector;

				return BestThr;
			}

			// If the hit ratio is enough, continue the iteration to reduce the error on negative samples.
			BestThr=ThrVal;
			if(FA)
				FA[0]=static_cast<double>(NegFail)/static_cast<double>(NegOK+NegFail);
			break;
		}

		// Store the new values only if they are better
		if(Mode!=THRF_HR && AuxErr<BestErr)
		{
			BestErr=AuxErr;
			BestThr=ThrVal;
		}
	}

	// Remove the auxiliary vector
	delete[] SortedVector;

	return BestThr;
}

void Evolutive::CCascade::Train(double MinHR,double MaxFA,double MaxStages,CABWeakLearner *WeakLearner,CSamplesSet *SampleSet,string OutFile)
{
	//CIntSamplesSet oInitialSampleSet,SampleSet;
	double FA=1.0,HR=0.0,*NewClasses=NULL;
	int *Labels=NULL;
	CvSize TrainingSize;
	double StageThr;
	bool OldFileExist;
	time_t Start_t, End_t;
	int ElapsedTime;

	// Temporal
	CAdaBoost oBoosting;

	// Parameters
	//string OutFile="Cascade.dat";
	//string SamplesPath= "../data/FaceDetect_01_Test.dat";	
	int NumPos;
	int NumNeg;
	double FinalFA=pow(MaxFA,MaxStages);
	
	// Load the initial sample set
	//SampleSet.Load(SamplesPath);
	if(!SampleSet->GetBinaryDist(NumPos,NumNeg))
	{
		throw CEvolutiveLibException("The training set is not binary",__FILE__,__LINE__,"Train");
	}

	// Use the samples set size as training size
	//TrainingSize=oInitialSampleSet.GetSamplesSize();
	TrainingSize=SampleSet->GetSamplesSize();

	// Show initial information
	cout << "Learning cascade" << endl;
	cout << "----------------" << endl;
	cout << "   Output file name: " << OutFile << endl;
	cout << "   Training size: " << TrainingSize.width << "x" << TrainingSize.height << endl;
	cout << "   Minimum Hit Ratio: " << MinHR << endl;
	cout << "   Maximum False Alarm: " << MaxFA << endl;
	cout << "   Positive samples: " << NumPos << endl;
	cout << "   Negative samples: " << NumNeg << endl;
	cout << "   Maximum number of stages: " << MaxStages << endl;

	// Configure the Adaboost object
	oBoosting.SetWeakLearner(WeakLearner);	

	// Allocate memory for the classes
	NewClasses=new double[NumPos+NumNeg];
	
	// Point to the samples labels
	Labels=WeakLearner->GetLabelsPtr();	

	// Look for the output file
	fstream ifs;
	ifs.open(OutFile.data(),ios_base::in | ios_base::binary);
	OldFileExist=!ifs.fail();
	ifs.close();

	// If the output file exists, load the cascade
	if(OldFileExist)
	{
		int PosOK,NegOK;
		
		// Load the classifier
		cout << "Loading previous stages...";
		Load(OutFile);
		cout << "   " << GetNumClassifiers() << " stages loaded" << endl;

		// Evaluate the samples
		WeakLearner->Classify(this,NewClasses);
		GetErrors(HR,FA,NumPos+NumNeg,NewClasses,Labels,0);
		cout << "   HR = " << HR << "   FA = " << FA << endl << endl;

		// If the cascade do not have the minimum HR performance, remove the last stages
		while(HR<MinHR)
		{
			cout << "Removing stage " << m_Classifiers.size() << endl;			
			m_Classifiers.pop_back();
			m_StageThresholds.pop_back();
			WeakLearner->Classify(this,NewClasses);
			GetErrors(HR,FA,NumPos+NumNeg,NewClasses,Labels,0);
			cout << "   HR = " << HR << "   FA = " << FA << endl << endl;
		}
	}
	
	// Begin adding stages
	while(GetNumClassifiers()<MaxStages && FA>FinalFA)
	{
		cout << endl << "Stage: " << GetNumClassifiers() << endl;

		// Prepare the samples
		try
		{
			WeakLearner->UpdateSamplesSet(this);
		}
		catch (Evolutive::CEvolutiveLibException *e) 
		{
			if(e->IsError("Not enough background files."))
			{
				cout << "Learning process stopped, insufficient background files." << endl;
				return;
			}
			else
			{
				cout << e->ToString() << endl;
				throw e;
			}
		}
		
		// Initialize the learner
		oBoosting.Initialize();

		FA=1.0;
		Start_t = time(NULL);
		while(FA>MaxFA)
		{
			// Add a new classifier to the ensemble
			oBoosting.AddClassifier();

			// Evaluate the samples
			WeakLearner->Classify(oBoosting.GetEnsemblePtr(),NewClasses);
			
			// Find a good threshold value
			StageThr=FindThreshold(NumPos+NumNeg,NewClasses,Labels,THRF_HR,&MinHR,&FA);
			GetErrors(HR,FA,NumPos+NumNeg,NewClasses,Labels,StageThr);
			cout << "   Thr = " << StageThr << "   HR = " << HR << "   FA = " << FA << endl;
		}
		End_t = time(NULL);
		ElapsedTime = difftime(End_t, Start_t);
		cout << "[" << ElapsedTime << " sec] Added " << oBoosting.GetEnsemble()->GetNumClassifiers() << "classifiers" << endl;

		// Add the new classifier
		AddClassifier(oBoosting.GetEnsemble(),StageThr);

		// Save the cascade to a file
		Save(OutFile);

		// Load the cascade again
		Load(OutFile);
	}

	// Release temporal memory
	delete[] NewClasses;
}

void Evolutive::CCascade::GetErrors(double &HR,double &FA,int NumSamples,double *Values,int *Labels,double Threshold)
{
	int PosOK,NegOK;
	int NumPos,NumNeg;
	double NewValue;
	register int i;

	// Initialize the errors
	PosOK=0;
	NegOK=0;
	NumPos=0;
	NumNeg=0;

	// Calculate the HR and FA	
	for(i=0;i<NumSamples;i++)
	{
		if(Labels[i]>0)
			NumPos++;
		if(Labels[i]<0)
			NumNeg++;
		NewValue=Values[i]<Threshold?-1.0:1.0;
		if(Labels[i]*NewValue>0 && Labels[i]>0)
			PosOK++;
		if(Labels[i]*NewValue>0 && Labels[i]<0)
			NegOK++;
	}
	HR=static_cast<double>(PosOK)/static_cast<double>(NumPos);
	FA=static_cast<double>(NumNeg-NegOK)/static_cast<double>(NumNeg);
}
#endif //USE_OPENCV